#include <QDebug>
#include <QtCore>

#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <cmath>

#include "smvprocess.h"
#include "pcap.h"

#define PI 3.1415927

#define ETH_BUFFER_LENGTH 1518

#define CAL_CYCLES 20

pcap_t* ethSocket = NULL;

QString etherPortName;

void pcap_setProtocolFilter(pcap_t* ethSocket, unsigned short etherType, char* dst_addr)
{
    char filterString[100];

    struct bpf_program etherTypeFilter;

    if(dst_addr == NULL)
    {
        sprintf(filterString, "(ether proto 0x%04x) or (vlan and ether proto 0x%04x)", etherType, etherType);
    }
    else
    {
        sprintf(filterString, "(ether dst host %s) and ( (ether proto 0x%04x) or (vlan and ether proto 0x%04x))", dst_addr, etherType, etherType);

    }

    if (pcap_compile(ethSocket, &etherTypeFilter, filterString, 1, 0) < 0) {
        qDebug()<< "Compiling packet filter failed!";
        return;
    }

    if (pcap_setfilter(ethSocket, &etherTypeFilter) < 0) {
        qDebug()<< "Setting packet filter failed!";
    }
    else
    {
        qDebug()<< "Setting packet filter Succeed!";
    }
}

QString getHexFormat(int iValue)
{
    QString sRes;

    sRes=QString::number(iValue,16);

    if(iValue < 0x10)
            sRes = "0" + sRes;
    return sRes;
}

smvProcess::smvProcess()
{
    qDebug()<<"hello smvSocket";

    init();
}

smvProcess::~smvProcess()
{
    closeEthSocket();
}

void smvProcess::init()
{
    flagClose = false;
    isOpened = false;
    run_mode = NOOP;
    deviceAddr = "";

    setChannel(smvProcess::Ua, 4);
    setChannel(smvProcess::Ub, 5);
    setChannel(smvProcess::Uc, 6);
    setChannel(smvProcess::Ia, 1);
    setChannel(smvProcess::Ib, 2);
    setChannel(smvProcess::Ic, 3);

    mu_sampleArray = &mu_PointArray1;
    mu_showArray = &mu_PointArray1;

    memset(mu_PointArray1,0,sizeof(mu_PointArray1));
    memset(mu_PointArray2,0,sizeof(mu_PointArray2));

    memset(lastOutput, 0x00, sizeof(lastOutput));
    memset(lastInput, 0x00, sizeof(lastInput));

    for(int i = 0; i < 10; i++)
    {
        dut_lists[i].isEmpty = true;
        dut_lists[i].packets_cnt = 0;
    }

    stopFlag = false;
}

void smvProcess::run()
{
    if(isOpened == false)
    {
        if(false == openEthSocket(1))
        {
            qDebug()<<"Open Smv Socket Error!";
            return;
        }
    }

    while(1)
    {
        if(isOpened == false)
        {
            qDebug()<<"Run finished!";
            return;
        }

        if (run_mode == POWER_BASICS)
        {
            findoutSamples();
        }
        else if (run_mode == GET_DEVICE_LIST)
        {
            listAllDevices();
        }
        else if(run_mode == TIME)
        {
            timeAnalyzer();
        }
        else
        {
            msleep(100);

            if(flagClose)
            {
                closeEthSocket();
                flagClose = false;
            }
        }

        if(reOpen == true)
        {
            reOpenEthSocket(seted_mode);
        }
    }
}

//void smvProcess::sleep()
//{
//    closeEthSocket();

//    //run_mode = NOOP;
//}

bool smvProcess::openEthSocket(int flag)
{
    char errbuf[PCAP_ERRBUF_SIZE];		// error buffer

    struct pcap_pkthdr *header;
    const u_char *pkt_data;
    int res;

#if 0
    ethSocket = pcap_open_offline("/home/luo/Desktop/9-2_packet/linchuang.pcap",errbuf);
#else
    qDebug()<<"eth port name:"<<etherPortName;
    //========================================================================
    // Open the port
    if ((ethSocket = pcap_open_live(etherPortName.toLatin1().data(),BUFSIZ,1,10,errbuf)) == NULL)
    {
        qDebug()<< "CapPacket::Open(), i can't open the port !";

        return false;
    }

//    if(flag == 1)
//    {
//        QByteArray ba = deviceAddr.toLatin1();
//        char* dst_addr = ba.data();

//        pcap_setProtocolFilter(ethSocket, 0x88ba, dst_addr);  //capture 9-2 SMV, and dest to MAC only
//    }
//    else
//    {
        pcap_setProtocolFilter(ethSocket, 0x88ba, NULL);  //capture 9-2 SMV, all devices

        qDebug()<<"===========================install fileter done!";
//    }
#endif
    int num_of_packets = 0;

    while(1)
    {
        res = pcap_next_ex(ethSocket, &header, &pkt_data);

        num_of_packets++;

        if ( (res < 1) || (header->len < 30) )
        {
            closeEthSocket();

            qDebug() << "smvProcess::Open(), error when reading from port!";

            return false;
        }

        if(num_of_packets == 10)
        {
            if( flag == 1)
            {
                setSmvPara(pkt_data, header->len);

                setChannelMapping();
            }

            qDebug()<< "smvProcess::Open(), Open Smv Socket succeed!";

            isOpened = true;

            return true;
        }
    }
}

void smvProcess::closeEthSocket()
{
    qDebug()<<"Try to close ethSocket!";

    if(isOpened == true)
    {
        isOpened = false;
        pcap_close(ethSocket);
    }

    return;
}

void smvProcess::setChannelMapping()
{
    qDebug()<<"do nothing...";
}

// return total length of this part
int berDecodeTL(const unsigned char* buffer, int* length)
{
    int berLength = *buffer;

//    qDebug()<<"berDecodeTL flag(hex):"<<hex<< berLength;

    if(berLength == 0x82)
    {
        *length = *(buffer + 1) * 256;
        *length += *(buffer + 2);

        return 3;
    }
    else if(berLength == 0x81)
    {
        *length = *(buffer + 1);

        return 2;
    }
    else
    {
        *length = *buffer;

        return 1;
    }
}

bool smvProcess::setSmvPara(const unsigned char* buffer, int len)
{
    int bufPos = 0;
    int length = 0;

    //ignor two MAC
    bufPos += 12;

    if (buffer[bufPos] == 0x81) // if has 802.1Q header
    {
        bufPos += 14;
    }
    else
    {
        bufPos += 10;
    }

    //qDebug()<<"Begin of APDU is here:"<< bufPos << hex <<(int)buffer[bufPos];

    bufPos++; // 0x60, start of APDU

    bufPos += berDecodeTL( buffer + bufPos , &length);

    bufPos++; // 0x80, start of number of ASDU

    bufPos++; // length of 0x80, always 0x01

    smvASDUCnt = buffer[bufPos];

    bufPos++;

    //qDebug()<< "ASDU Cnt:"<< smvASDUCnt;

    if(buffer[bufPos] == 0xa2) //9-2 LE
    {
        bufPos++;
        bufPos += berDecodeTL( buffer + bufPos , &length);
    }
    else
    {
        // more parts besides smv_asdu_count
        while (1)
        {
            if(buffer[bufPos++] == 0xa2)
            {
                 bufPos += berDecodeTL( buffer + bufPos , &length);
                 break;
            }

            if(bufPos > len)
            {
                return false;
            }
        }
    }

    bufPos++; // 0x30

    bufPos += berDecodeTL( buffer + bufPos , &length);

    smvASDULength = length;

    //qDebug()<<"Length of one smvASDU (bytes):"<< smvASDULength;

    if( 0x80 == buffer[bufPos]) //svID
    {
        bufPos++;
        bufPos += buffer[bufPos];
        bufPos++;
    }

//    qDebug()<< hex<<(int)buffer[bufPos];

    if( 0x81 == buffer[bufPos]) //DataSet
    {
        bufPos++;
        bufPos += berDecodeTL( buffer + bufPos , &length);
        bufPos += length;
    }

    if( 0x82 == buffer[bufPos]) //SampleCnt
    {
        bufPos++;
        smvSmpCntLength = buffer[bufPos];

        bufPos++;
        smvFirstSmpCntStart = bufPos;

        bufPos++;
        bufPos++;
    }

    if( 0x83 == buffer[bufPos]) // version of cfg
    {
        bufPos++;
        bufPos += berDecodeTL( buffer + bufPos , &length);
        bufPos += length;
    }

    if( 0x84 == buffer[bufPos]) //refresh time
    {
        bufPos++;
        bufPos += berDecodeTL( buffer + bufPos , &length);
        bufPos += length;
    }

    if( 0x85 == buffer[bufPos])
    {
        bufPos++;
        bufPos++;

        if( 0 == buffer[bufPos])
        {
            smvSyncStatus = false;
        }
        else
        {
            smvSyncStatus = true;
        }

        bufPos++;
    }

    if( 0x86 == buffer[bufPos]) //sample rate
    {
        bufPos++;
        bufPos += berDecodeTL( buffer + bufPos , &length);
        bufPos += length;
    }

    if( 0x87 == buffer[bufPos])
    {
        bufPos++;
        bufPos += berDecodeTL( buffer + bufPos , &length);

        smvPosOfFirstSample = bufPos;

        smvNumOfChannels = length / 8;

        //qDebug()<<"Number of channels:"<<smvNumOfChannels;
    }

    return true;
}

double digiFiter(int flag, double newComing)
{
    static double filterBuff[9][6]={{0}};

    double *dataArray = &filterBuff[flag][0];
    double sum, max, min;

    if(dataArray[0] == 0)
    {
        dataArray[0] = dataArray[1] = dataArray[2] = dataArray[3] = dataArray[4] = dataArray[5] = newComing;
    }
    else
    {
        dataArray[5] = newComing;
    }

    sum = max = min = dataArray[0];

    for(int i = 1; i < 6; i++)
    {
        double tmp = dataArray[i];

        dataArray[i-1] = tmp;

        sum += tmp;

        if(tmp > max) max = tmp;
        if(tmp < min) min = tmp;
    }

    sum = sum - max - min;

    return sum/4.0;
}

double smvProcess::digiFiter_dcBlocking(int flag, double newComing)
{
    lastOutput[flag] = newComing - lastInput[flag] + 0.995*lastOutput[flag];
    lastInput[flag] = newComing;

    return lastOutput[flag];
}

/*
typedef struct{
    unsigned char	index; // default 0, set it 1 when we find the first device and 2 for second...
    unsigned char	dest_addr[6];
    unsigned int	appid;
    unsigned int	smp_rate;	//in k bits

    unsigned int	smps_cnt;
} dut;

dut	dut_lists[10];	// list ten devices at most
*/
void smvProcess::listAllDevices()
{
    QString m_sAPPID="";
    QString m_sMAC="";
    QString m_sType="";

    unsigned int    appid_tmp;
    unsigned int    iSmpCnt_tmp = 0;
    unsigned char	dest_addr_tmp[6];

    struct pcap_pkthdr *header;
    const u_char *pkt_data;

    int packetSize;
    u_char buff[ETH_BUFFER_LENGTH] = {0};

    if(stopFlag == true)
    {
        closeEthSocket();
        return;		// stop listing
    }

    int pcapCode = pcap_next_ex(ethSocket, &header, &pkt_data);

    if (pcapCode > 0) {

        packetSize = header->caplen;

        if (packetSize > ETH_BUFFER_LENGTH)
        {
                packetSize = ETH_BUFFER_LENGTH;
        }

        memcpy(buff, pkt_data, packetSize);
    }
    else
    {
        qDebug()<< " cap packet error! close the EthSocket!";

        closeEthSocket();

        return;
    }

    int i, j;

    for(j = 0; j < 6; j++)
    {
        dest_addr_tmp[j] = buff[j];
    }

    for(i = 0; i <= packetSize; i++)
    {
        if ( (buff[i] == 0x88) && (buff[i + 1] == 0xba) ) break;
    }

    appid_tmp = buff[i + 2] * 256 + buff[i + 3];

    m_sAPPID = getHexFormat(buff[i + 2]) + getHexFormat(buff[i + 3]);

    setSmvPara(buff, packetSize);

    //qDebug()<< "m_iASDU_Cnt, m_iFirstSmpCnt, mASDU_Length"<< m_iASDU_Cnt << m_iFirstSmpCnt_Start <<m_iASDU_Length;

    iSmpCnt_tmp = 0;

    for(j = 0; j < smvASDUCnt; j++)
    {
        int smvSmpCntStart = smvFirstSmpCntStart + j * (smvASDULength);

        for(int i = 0; i < smvSmpCntLength; i++)
            iSmpCnt_tmp += (buff[smvSmpCntStart + i] << (8 * (smvSmpCntLength - i -1)) );
    }

//   qDebug()<<"listDevice(), iSmpCnt_tmp="<<iSmpCnt_tmp;

    for(i = 0; i < 10; i++)
    {
        bool	dut_is_in_list = false;

        if(! dut_lists[i].isEmpty)
        {
            for(j = 0; j < 6; j++) // if mac addr is matching
            {
                if(dut_lists[i].dest_addr[j] == dest_addr_tmp[j])
                {
                    dut_is_in_list = true;
                }
                else
                {
                    dut_is_in_list = false;
                    break;
                }
            }

            if(dut_is_in_list) //if appid is matching
            {
                if(dut_lists[i].appid != appid_tmp)
                {
                    dut_is_in_list = false;
                }
                else
                {
                    // find dut in the list, go out this loop
                    break;
                }
            }
        }
        else    //if the dut_lists[i] is empty, dut is not in the list
        {
            for(j = 0; j < 6; j++)	dut_lists[i].dest_addr[j] = dest_addr_tmp[j];

            dut_lists[i].appid = appid_tmp;
            dut_lists[i].isEmpty = false;

            // add dut into list and go out loop
            break;
        }
    }

    if(dut_lists[i].packets_cnt < 5000)
    {
        dut_lists[i].packets_cnt++;

        if(dut_lists[i].smp_rate < iSmpCnt_tmp) dut_lists[i].smp_rate = iSmpCnt_tmp;
    }
    else if(dut_lists[i].packets_cnt == 5000)	// we do nothing when smps_cnt = 5001
    {
        if(dut_lists[i].smp_rate > 4000)	// if smp_cnt > 4000 is found
        {
            dut_lists[i].smp_rate = 12800;
        }
        else
        {
            dut_lists[i].smp_rate = 4000;
        }

        for(j = 0; j < 6; j++)
        {
            m_sMAC += getHexFormat(dest_addr_tmp[j]) +"-";
        }

        m_sMAC = m_sMAC.left(m_sMAC.length()-1);

        m_sType = "9-2";

        emit sigFind(m_sAPPID, m_sMAC, m_sType, dut_lists[i].smp_rate, smvASDUCnt);         //show this device in the list

        qDebug()<<"appid, maxSmpCnt, asduCnt"<<m_sAPPID<<dut_lists[i].smp_rate<<smvASDUCnt;

        dut_lists[i].packets_cnt++;
    }
}

void smvProcess::findoutSamples()
{
    static double dc_cnt[6]={0};

    struct pcap_pkthdr *header;	// 数据包头
    const u_char *pkt_data;		// 数据包
    int packetSize;

    int sampleValue;

    int sampleIndex = 0;

    u_char buff[ETH_BUFFER_LENGTH] = {0};

    if(!isOpened)
    {
        return;
    }

    int pcapCode = pcap_next_ex(ethSocket, &header, &pkt_data);

    if (pcapCode > 0) {

        packetSize = header->caplen;

        if (packetSize > ETH_BUFFER_LENGTH)
        {
                packetSize = ETH_BUFFER_LENGTH;
        }

        memcpy(buff, pkt_data, packetSize);
    }
    else
    {
        qDebug()<< " cap packet error! close the EthSocket!";

        closeEthSocket();

        return;
    }

    for(int i = 0; i < smvASDUCnt; i++)
    {
        sampleIndex = 0;

        smvSmpCntStart = smvFirstSmpCntStart + i * smvASDULength;

        for(int j = 0; j < smvSmpCntLength; j++)
        {
            sampleIndex += (buff[smvSmpCntStart + j] << (8 * (smvSmpCntLength - j -1)) );
        }
        
        if(sampleIndex < 80 * CAL_CYCLES)
        {
            for(int channel = 0; channel < smvNumOfChannels; channel++)
            {
                int posOfSamples = smvPosOfFirstSample + i * smvASDULength + 8 * channel;

                // rebuild the int32 from net long( ntohl(), Little Endian )
                sampleValue =  buff[posOfSamples + 3]
                            | (buff[posOfSamples + 2] << 8)
                            | (buff[posOfSamples + 1] << 16)
                            | (buff[posOfSamples] << 24);

                sampleValue += 0.000000001;

                int realChannel = smvNumOfChannels * i + channel + 1;

                double value_tmp = 0;

                if(realChannel == channelList[Ua])
                {
                    //sampleValue = digiFiter_dcBlocking(Ua, sampleValue);

                    value_tmp = sampleValue / 100.0;

                    (*mu_sampleArray)[Ua][sampleIndex] = value_tmp;

                    dc_cnt[Ua] += value_tmp;

                    //rms_Ua_count += value_tmp * value_tmp;

                    //qDebug()<<"sample of Ua"<< value_tmp<< QString::number(value_tmp,'f',2);
                }

                if(realChannel == channelList[Ub])
                {
                    //sampleValue = digiFiter_dcBlocking(Ub, sampleValue);

                    value_tmp = sampleValue / 100.0;

                    (*mu_sampleArray)[Ub][sampleIndex] = value_tmp;

                    dc_cnt[Ub] += value_tmp;

                    //rms_Ub_count += value_tmp * value_tmp;

                    ///qDebug()<<"sample of Ub"<< value_tmp<< QString::number(value_tmp,'f',2);
                }

                if(realChannel == channelList[Uc])
                {
                    //sampleValue = digiFiter_dcBlocking(Uc, sampleValue);

                    value_tmp = sampleValue / 100.0;

                    (*mu_sampleArray)[Uc][sampleIndex] = value_tmp;

                    dc_cnt[Uc] += value_tmp;

                    //rms_Uc_count += value_tmp * value_tmp;

                    ///qDebug()<<"sample of Uc"<< value_tmp<< QString::number(value_tmp,'f',2);
                }

                if(realChannel == channelList[Ia])
                {
                    //sampleValue = digiFiter_dcBlocking(Ia, sampleValue);

                    value_tmp = sampleValue / 1000.0;

                    (*mu_sampleArray)[Ia][sampleIndex] = value_tmp;

                    dc_cnt[Ia] += value_tmp;

                    //rms_Ia_count += value_tmp * value_tmp;

                    //qDebug()<<"sample of Ia"<< value_tmp<< QString::number(value_tmp,'f',2);
                }

                if(realChannel == channelList[Ib])
                {
                    //sampleValue = digiFiter_dcBlocking(Ib, sampleValue);

                    value_tmp = sampleValue / 1000.0;

                    (*mu_sampleArray)[Ib][sampleIndex] = value_tmp;

                    dc_cnt[Ib] += value_tmp;

                    //rms_Ib_count += value_tmp * value_tmp;

                    //qDebug()<<"sample of Ib"<< value_tmp<< QString::number(value_tmp,'f',2);
                }

                if(realChannel == channelList[Ic])
                {
                    //sampleValue = digiFiter_dcBlocking(Ic, sampleValue);

                    value_tmp = sampleValue / 1000.0;

                    (*mu_sampleArray)[Ic][sampleIndex] = value_tmp;

                    dc_cnt[Ic] += value_tmp;

                    //rms_Ic_count += value_tmp * value_tmp;

                    //qDebug()<<"sample of Ic"<< value_tmp<< QString::number(value_tmp,'f',2);
                }
            }
        }

        if(sampleIndex == 80 * CAL_CYCLES - 1)
        {
            // ping-pong buffer
            if(mu_sampleArray == &mu_PointArray1)
            {
                mu_sampleArray = &mu_PointArray2;
                mu_showArray = &mu_PointArray1;
            }
            else
            {
                mu_sampleArray = &mu_PointArray1;
                mu_showArray = &mu_PointArray2;
            }

            qDebug()<<"Ua";
            harmonic((*mu_showArray)[Ua],1,800);
            rms_Ua = dbl_rms;
            dbl_frequency_Ua = dbl_frequency_tmp;
            dbl_phase_Ua = dbl_phase;
            dc_level[Ua] = dc_cnt[Ua] / 800;

            qDebug()<<"Ub";
            harmonic((*mu_showArray)[Ub],1,800);
            rms_Ub = dbl_rms;
            dbl_frequency_Ub = dbl_frequency_tmp;
            dbl_phase_Ub = dbl_phase;
            dc_level[Ub] = dc_cnt[Ub] / 800;

            qDebug()<<"Uc";
            harmonic((*mu_showArray)[Uc],1,800);
            rms_Uc = dbl_rms;
            dbl_frequency_Uc = dbl_frequency_tmp;
            dbl_phase_Uc = dbl_phase;
            dc_level[Uc] = dc_cnt[Uc] / 800;

            qDebug()<<"Ia";
            harmonic((*mu_showArray)[Ia],1,800);
            rms_Ia = dbl_rms;
            dbl_phase_Ia = dbl_phase;
            dc_level[Ia] = dc_cnt[Ia] / 800;

            qDebug()<<"Ib";
            harmonic((*mu_showArray)[Ib],1,800);
            rms_Ib = dbl_rms;
            dbl_phase_Ib = dbl_phase;
            dc_level[Ib] = dc_cnt[Ib] / 800;

            qDebug()<<"Ic";
            harmonic((*mu_showArray)[Ic],1,800);
            rms_Ic = dbl_rms;
            dbl_phase_Ic = dbl_phase;
            dc_level[Ic] = dc_cnt[Ic] / 800;

            emit samplesIsReady();
        }
    }
}

void smvProcess::reOpenEthSocket(_run_mode mode)
{
    closeEthSocket();

    if(mode == POWER_BASICS)
    {
        run_mode = POWER_BASICS;

        openEthSocket(1);
    }
    else if(mode == GET_DEVICE_LIST)
    {
        run_mode = GET_DEVICE_LIST;

        for(int i = 0; i < 10; i++)
        {
            dut_lists[i].isEmpty = true;
            dut_lists[i].packets_cnt = 0;
        }

        openEthSocket(0);
    }
    else if(mode == TIME)
    {
        isFirstCycle = true;

        run_mode = TIME;

        openEthSocket(0);
    }

    reOpen = false;
}

void smvProcess::harmonic(double *s,int M,int len)   //s 所有的点 M：谐波次数（可以不用）len smrate*zhouqi fs smrate*50
{
    int fs = 4000;
    int p,n,j,k;
    double a,b;
    int f0;
    double y,y2,maxu;
    long k0;

    f0=50;
    double r[len];
    memset(r,0,len);

    for(int i = 0; i < len ; i++)
    {
        r[i]=s[i] *( 0.3125 - 0.46875 * cos ( 2 * PI * i / len )
                    + 0.1875 * cos( 4 * PI * i / len)
                    - 0.03125 * cos( 6 * PI * i / len) );//FDMS_1 Window
    }

    int tm = len * M * 50 / fs + 4;//M=1,tm=14;M=40,tm=404;

    double L[tm];
    double T[tm];

    // assume that tm cycles of signal is included in s[len] at most
    for(int i = 0; i < tm; i++)
    {
        L[i]=0;
        T[i]=0;

        for( n = 0 ; n < len ; n++)
        {
            T[i] = T[i] + r[n] * cos( 2 * PI * i * n / len);
            L[i] = L[i] + r[n] * sin( 2 * PI * i * n / len);
        }
    }

    double u[tm];
    for(p=0; p<tm; p++)
    {
        u[p]=std::sqrt(L[p]*L[p] + T[p]*T[p]);
    }

    //if the xa(t)'s frequency is f0, find k0
    k0 = std::ceil(f0*len/fs);
    maxu = u[k0];
    k = k0;

    //if there is a spectrum more larger than k0
    for(j=k0-3;j<k0+3;j++)
    {
        if(maxu<u[j])
        {
            maxu=u[j];//first high of freq spectrum
            k=j;
        }
    }

    //second high of freq	spectrum, left or right of maxu
    if(u[k-1]>u[k+1])  k=k-1;

    y=u[k];
    y2=u[k+1];
    b=(y2-y)/(y2+y);
    a=3.5*b;

    //dertermine the k_real by maxu and maxu_second
    dbl_frequency_tmp = (k+a+0.5)*fs/len;//freq of signal

    dbl_rms=(y+y2)*(3.43612+0.85434*std::pow(a,2)
                    + 0.11871*std::pow(a,4))
                    / len
                    / 1.4142135623;//RMS of signal

    if(T[k]>0)
        dbl_phase=std::acos(L[k]/u[k])-(a+0.5)*PI;//phase of angel,range of angel is 0-2*PI
    else if(T[k]<0)
        dbl_phase=2*PI-std::acos(L[k]/u[k])-(a+0.5)*PI;
    else if(T[k]==0)
    {
         if(L[k]>=0)   dbl_phase=-(a+0.5)*PI;
         else    dbl_phase=PI-(a+0.5)*PI;
    }

    if(dbl_phase > 2*PI)
        dbl_phase -= 2 * PI;

    if(dbl_phase <= 0)
        dbl_phase += 2 * PI;

    qDebug()<<"freq, phase, rms"<<dbl_frequency_tmp<<dbl_phase*180/PI<<dbl_rms;
}

void smvProcess::timeAnalyzer()
{
    static int sampleIndexOld = 0;
    static double time_stamp_old = 0;
    static double time_stamp_zero = 0;

    double time_stamp = 0;

    struct pcap_pkthdr *header;	// 数据包头
    const u_char *pkt_data;		// 数据包
    int packetSize;

    int sampleIndex = 0;

    u_char buff[ETH_BUFFER_LENGTH] = {0};

    if(!isOpened)
    {
        return;
    }

    int pcapCode = pcap_next_ex(ethSocket, &header, &pkt_data);

    if (pcapCode > 0) {

        packetSize = header->caplen;

        if (packetSize > ETH_BUFFER_LENGTH)
        {
            packetSize = ETH_BUFFER_LENGTH;
        }

        memcpy(buff, pkt_data, packetSize);
    }
    else
    {
        qDebug()<< " cap packet error! close the EthSocket!";

        closeEthSocket();

        return;
    }

    // find out the time_stamp
    unsigned int count = 0;
    for(int i = 0; i < 4; i++)
        count += (unsigned int)pkt_data[header->caplen - i - 1] << (8 * i);
    time_stamp = count * 4/ 100.0;

    int posOfSamples = smvPosOfFirstSample;

    // rebuild the int32 from net long( ntohl(), Little Endian )
    test_result_tmp.delay_defined =  buff[posOfSamples + 3]
                | (buff[posOfSamples + 2] << 8)
                | (buff[posOfSamples + 1] << 16)
                | (buff[posOfSamples] << 24);

    sampleIndex = 0;

    for(int j = 0; j < smvSmpCntLength; j++)
    {
        sampleIndex += (buff[smvFirstSmpCntStart + j] << (8 * (smvSmpCntLength - j -1)) );
    }

    //test_result_tmp.total++;

    qDebug()<<"---\nsampleIndex="<<sampleIndex <<sampleIndexOld<<"time_stamp, time_stamp_old\n--->"<< time_stamp<< time_stamp_old;

    if( sampleIndex == 0 )
    {
        if(isFirstCycle)
        {
            memset(&test_result_tmp, 0x00, sizeof(test_result_tmp));

            time_stamp_old = time_stamp;
            sampleIndexOld = sampleIndex;

            isFirstCycle = false;
            qDebug()<<"begin test";
        }
        else
        {
            test_result_tmp.total += 4000;

            memcpy(&test_result, &test_result_tmp, sizeof(test_result));
            test_result.delay = time_stamp * 100;
            test_result.lost = test_result.total -test_result.error_1 - test_result.error_2 - test_result.error_4 - test_result.error_6;

            time_stamp_old = time_stamp;
            sampleIndexOld = sampleIndex;

            emit samplesIsReady();

            //qDebug()<<"packet lost"<<test_result.lost;
        }

        test_result_tmp.error_1++;

        return;
    }

    if( sampleIndex == sampleIndexOld + 1)
    {
        time_stamp_old = fabs( time_stamp - time_stamp_old - 250 );

        if(time_stamp_old < 2)
        {
            test_result_tmp.error_1++;
        }
        else if(time_stamp_old < 4)
        {
            test_result_tmp.error_2++;
        }
        else if(time_stamp_old < 8)
        {
            test_result_tmp.error_4++;
        }
        else
        {
            if(time_stamp < 250)
            {
                time_stamp_zero = time_stamp;

                test_result_tmp.error_1++;
            }
            else
            {
                test_result_tmp.error_6++;
#ifdef i386
                qDebug()<<"\n--- delay is too big";
                qDebug()<<"--->sampleIndex"<<sampleIndex<<"sampleIndexOld"<<sampleIndexOld;
                qDebug()<<"--->delay"<<time_stamp_old<<"timeStamp_old"<<time_stamp;
#endif
            }
        }

        time_stamp_old = time_stamp;

        sampleIndexOld = sampleIndex;
    }
    else if(sampleIndexOld >  sampleIndex)
    {   
        int time_jetter = fabs( time_stamp - time_stamp_old - 250 );

        if(time_jetter < 230)
        {

#ifndef i386
            if(sampleIndex == 257) return;
#endif

            test_result_tmp.disorder++;

            time_stamp_old = time_stamp;
#ifdef i386
            qDebug()<<"disorder is found: sampleIndexOld"<<sampleIndexOld<<"sampleIndex"<<sampleIndex;
#endif
        }
    }
    else
    {
        time_stamp_old = time_stamp;
        sampleIndexOld = sampleIndex;

        //qDebug()<<"packet lost happed!";


        //qDebug()<<"---\nsampleIndex="<<sampleIndex <<sampleIndexOld<<"time_stamp, time_stamp_old\n--->"<< time_stamp<< time_stamp_old;
    }
}
